//
// Created by skywoodsz on 2023/2/28.
//
// copy from: https://github.com/qiayuanliao/legged_control

#ifndef SRC_SAFETYCHECKER_H
#define SRC_SAFETYCHECKER_H

#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_mpc/SystemObservation.h>

namespace qm{
using namespace ocs2;
using namespace centroidal_model;

class SafetyChecker {
public:
    explicit SafetyChecker(const CentroidalModelInfo& info) : info_(info) {}

    bool check(const SystemObservation& observation, const vector_t& /*optimized_state*/, const vector_t& /*optimized_input*/) {
        return checkOrientation(observation);
    }

protected:
    bool checkOrientation(const SystemObservation& observation) {
        vector_t pose = getBasePose(observation.state, info_);
        if (pose(5) > M_PI_2 || pose(5) < -M_PI_2) {
            std::cerr << "[SafetyChecker] Orientation safety check failed!" << std::endl;
            return false;
        }
        return true;
    }

    const CentroidalModelInfo& info_;
};
}


#endif //SRC_SAFETYCHECKER_H
